﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Diagnostics;
using System.Threading;
using System.IO;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningAlgorithms;
using MotionPlanning.MotionPlanningAlgorithms.RRTstar;
using MotionPlanning.MotionPlanningAlgorithms.RRT_Standard;
using MotionPlanning.MotionPlanningAlgorithms.ioRRT_Prototype;
using MotionPlanning.MotionPlanningAlgorithms.RRTas_Standard;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningExperiments;

namespace MotionPlanning
{
    public partial class Form1 : Form, UIForm
    {
        public Form1()
        {
            InitializeComponent();
            this.FormClosing += new FormClosingEventHandler(Form1_FormClosing);
        }

        void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if(currentExperiment != null)
                currentExperiment.Purge();
        }
        
        private Experiment currentExperiment;

        void CanvasRegion_Paint(object sender, PaintEventArgs e)
        {
            currentExperiment.FlushCanvas_primary();
        }

        private void button43_Click(object sender, EventArgs e)
        {
            currentExperiment.Start();
        }

        public Button get_button_start()
        {
            return button_start;
        }

        public RichTextBox get_console_box()
        {
            return richTextBox1;
        }

        public RichTextBox get_monitor_box()
        {
            return richTextBox2;
        }

        public Graphics get_canvas_primary()
        {
            return CanvasRegion.CreateGraphics();
        }

        private void button43_Click_1(object sender, EventArgs e)
        {
            currentExperiment.FlushCanvas_primary();
        }

        private void button2_Click(object sender, EventArgs e)
        {
            RoboticSystemComponent system = new System_Point2D();

            Visualizer_Point2D visual = new Visualizer_Point2D(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Point2D_ioRRT.kdl");
            currentExperiment = new Experiment_Multithreading("Point2D_ioRRT", 6, planner, this, 1500000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button3_Click(object sender, EventArgs e)
        {
            RoboticSystemComponent system = new System_Car1st();

            Visualizer_Car1st visual = new Visualizer_Car1st(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            visual.Set_ActiveArea_Primary(CanvasRegion);
            //Planner_RRTBased planner = new ioRRT(system, visual, "c:/tmp/MotionPlanningResources/Car1st_test.kdl");
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Car1st_ioRRT.kdl");
            currentExperiment = new Experiment_Multithreading("Car1st_ioRRT", 6, planner, this, 1500000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button4_Click(object sender, EventArgs e)
        {
            System_Impreza system = new System_Impreza();

            Visualizer_Impreza visual = new Visualizer_Impreza(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            visual.Set_ActiveArea_Primary(richTextBox2);
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Impreza_ioRRT.kdl");
            ((ioRRT)planner).SetGoalCheck(true);
            currentExperiment = new Experiment_Multithreading("Impreza_ioRRT", 1, planner, this, 3000000);

            CanvasRegion.Paint += new PaintEventHandler(CanvasRegion_Paint);
        }

        private void button1_Click(object sender, EventArgs e)
        {

            System_Impreza system = new System_Impreza();

            Visualizer_RallyCar visual = new Visualizer_RallyCar(
                CanvasRegion.CreateGraphics(),
                new Rectangle(0, 0, CanvasRegion.Width, CanvasRegion.Height));
            visual.Set_ActiveArea_Primary(richTextBox2);
            Planner_RRTBased planner = new ioRRT(system, visual, "../../MotionPlanningResources/Impreza_ioRRT.kdl");
            ((ioRRT)planner).SetGoalCheck(true);
            currentExperiment = new Experiment_Multithreading("Impreza_ioRRT", 1, planner, this, 1500000);

            double[] q1 = new double[]{
                -7.6529168390766991,
                20.042968391681249,
                3.9421055142152168,
                -0.58169909199894854,
                -2.0679275516036322,
                0.70196778816296224,
                11.74830321819366,
                5.85441840872425
            };

            double[] q2 = new double[q1.Length];
            double[] u = new double[] {
                0.43651785068759597,
                -677.508244513305,
                -213.21788845267974,
                0.13966987973063713
            };
            double t = u[3];
            double cost = 0;
            system.Propagate(q1, u, t, ref q2, ref cost);



        }

    }


}
